function [X_guess, U_guess] = align_initial_guess(last_res, qs_info)
% ALIGN_INITIAL_GUESS 对齐历史优化结果到当前时间窗口
% 输入:
%   last_res - 上一次优化结果 [t_ v_ms_opt s_opt Ts_opt Tr_opt Fe_opt Fm_opt]
%   qs_info  - 当前优化配置结构体（含 t_now, set_time 等）
%   N        - 优化时域步数
%   dt       - 当前时间步长
% 输出:
%   X_guess  - 状态变量初始猜测 (4 x N)
%   U_guess  - 控制变量初始猜测 (2 x N)

% 生成时间向量
td_ = qs_info.t_now : qs_info.step_t : (qs_info.t_now + qs_info.set_time + qs_info.t_err);
N = length(td_);
dt = qs_info.step_t;
% 默认初始猜测（零初始化）
X_guess = zeros(4, N);
U_guess = zeros(2, N);

% 如果没有历史数据，直接返回零初始值
if isempty(last_res)
    return;
end

%% 1. 时间对齐与数据截取
% 当前优化时间窗口
t_start = qs_info.t_now;
t_end = t_start + qs_info.set_time + qs_info.t_err;

% 提取历史数据中时间在 [t_start, t_end] 范围内的部分
valid_idx = last_res(:,1) >= t_start & last_res(:,1) <= t_end;
if ~any(valid_idx)
    return;  % 无有效历史数据
end
t_hist = last_res(valid_idx, 1);
v_hist = last_res(valid_idx, 2);
s_hist = last_res(valid_idx, 3);
Ts_hist = last_res(valid_idx, 4);
Tr_hist = last_res(valid_idx, 5);
Fe_hist = last_res(valid_idx, 6);
Fm_hist = last_res(valid_idx, 7);

% 转换为相对于当前 t_start 的时间
t_hist_aligned = t_hist - t_start;

%% 2. 生成当前时间网格并插值
t_current = (0:N-1)*dt;  % 当前优化时间网格（从0开始）

% 线性插值（处理超出范围的点用末值填充）
v_guess = interp1(t_hist_aligned, v_hist, t_current, 'linear');
s_guess = interp1(t_hist_aligned, s_hist, t_current, 'linear');
Ts_guess = interp1(t_hist_aligned, Ts_hist, t_current, 'linear');
Tr_guess = interp1(t_hist_aligned, Tr_hist, t_current, 'linear');
Fe_guess = interp1(t_hist_aligned, Fe_hist, t_current, 'linear');
Fm_guess = interp1(t_hist_aligned, Fm_hist, t_current, 'linear');

%% 3. 构造初始猜测矩阵
X_guess = [v_guess(1:N);  % 速度
           s_guess(1:N);  % 位置
           Ts_guess(1:N); % 温度Ts
           Tr_guess(1:N)];% 温度Tr

U_guess = [Fe_guess(1:N); % 控制力Fe
           Fm_guess(1:N)];% 控制力Fm

% 异常处理：插值失败时用末值填充
if any(isnan(X_guess(:))) || any(isnan(U_guess(:)))
    last_X = [v_hist(end); s_hist(end); Ts_hist(end); Tr_hist(end)];
    last_U = [Fe_hist(end); Fm_hist(end)];
    X_guess(:, isnan(X_guess(1,:))) = repmat(last_X, 1, sum(isnan(X_guess(1,:))));
    U_guess(:, isnan(U_guess(1,:))) = repmat(last_U, 1, sum(isnan(U_guess(1,:))));
end
end